#! /usr/bin/env python
import cv2
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
import rospy
from std_msgs.msg import String,Int32MultiArray
from sensor_msgs.msg import Image

class Match:
    def __init__(self):
        self.bridge = CvBridge()
        self.img_sub = rospy.Subscriber('/camera/rgb/image_raw',Image,self.imgCallback)
        self.location_pub = rospy.Publisher('/camera/loc',Int32MultiArray,queue_size=10)
        self.template = cv2.imread("image_coke.png")
        self.std = 0
        self.loc = [0,0,1,1]
        #print("h1={0}, w1={0}".format(h1, w1))
        #self.template = cv2.resize(self.template, (int(h1*1), int(w1*1)))
        # self.match(1)

    def imgCallback(self,data):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data,'bgr8')
            self.std = 0
            # cv2.imshow('src',cv_image)
            # cv2.waitKey(1)
            self.match(cv_image)
            if self.std == 1:
                print('capture')
                msg = Int32MultiArray(data = self.loc)
                # print(msg)
                self.location_pub.publish(msg)

        except:
            print('error')


    def match(self, data):
        img = data
        string1 = "../photos/image"
        string2 = "C.png"
        scores = []
        for i in range(3):
            imagepath = string1+str(i)+string2
            self.template = cv2.imread(imagepath)
            result = cv2.matchTemplate(img, self.template, 3)
            min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(result)
            top_left = max_loc
            bottom_right = (top_left[0] + self.template.shape[1], top_left[1] + self.template.shape[0])
            datas = result[top_left[1]:(top_left[1] + self.template.shape[0]),top_left[0]:(top_left[0] + self.template.shape[1])]
            q = sum(sum(datas))/(self.template.shape[0]*self.template.shape[1])
            l = len(scores)
            if l>0:
                if q>scores[l-1]:
                    scores.append(q)
                    self.loc[0] = top_left[0]
                    self.loc[1] = top_left[1]
                    self.loc[2] = bottom_right[0]
                    self.loc[3] = bottom_right[1]
            else:
                scores.append(q)
                self.loc[0] = top_left[0]
                self.loc[1] = top_left[1]
                self.loc[2] = bottom_right[0]
                self.loc[3] = bottom_right[1]
        print(scores[-1])
        # if scores[-1] > 0.03 or scores[-1] < -0.03:
        cv2.rectangle(img, tuple(self.loc[0:2]), tuple(self.loc[2:4]), (255, 0, 0), 2)
        cv2.imshow("img", img)
        if cv2.waitKey(1) == ord('c'):
            self.std = 1

def main():
    rospy.init_node('select_target',anonymous = False)
    match = Match()
    rospy.spin()

if __name__ == "__main__":
    try:
        main()
    except rospy.ROSInterruptException:
        pass
